use serde::Serialize;

pub const MIN_SIZE: usize = 0xEA;

#[derive(Clone, Debug, Serialize)]
pub struct StateResp {
    program_state: u8,
    error_code: u8,
    robot_mode: u8,
    
    jt_cur_pos0: f64,
    jt_cur_pos1: f64,
    jt_cur_pos2: f64,
    jt_cur_pos3: f64,
    jt_cur_pos4: f64,
    jt_cur_pos5: f64,

    tl_cur_pos0: f64,
    tl_cur_pos1: f64,
    tl_cur_pos2: f64,
    tl_cur_pos3: f64,
    tl_cur_pos4: f64,
    tl_cur_pos5: f64,
    
    tool_num: u32,

    jt_cur_tor0: f64,
    jt_cur_tor1: f64,
    jt_cur_tor2: f64,
    jt_cur_tor3: f64,
    jt_cur_tor4: f64,
    jt_cur_tor5: f64,

    program_name: String,
    prog_total_line: u8,
    prog_cur_line: u8,

    cl_dgt_output_h: u8,
    cl_dgt_output_l: u8,
    tl_dgt_output_l: u8,
    cl_dgt_input_h: u8,
    cl_dgt_input_l: u8,
    tl_dgt_input_l: u8,

    ft_data0: f64,
    ft_data1: f64,
    ft_data2: f64,
    ft_data3: f64,
    ft_data4: f64,
    ft_data5: f64,

    ft_act_status: u8,

    emergency_stop: u8,
    robot_motion_done: u32,
    gripper_motion_done: u8,
}

fn decode_cstr(buf: &[u8], len: usize) -> String {
    let mut s = String::new();
    for v in &buf[..len] {
        if *v == 0 {
            break;
        }
        s.push( (*v) as char );
    }
    s
}

pub mod le {
 
    pub fn decode_u8(buf: &[u8]) -> u8 {
        buf[0]
    }
    
    pub fn decode_u16(buf: &[u8]) -> u16 {
        let v0 = buf[0] as u16;
        let v1 = buf[1] as u16;
        (v1 << 8) + 
        v0
    }
    
    pub fn decode_u32(buf: &[u8]) -> u32 {
        let v0 = buf[0] as u32;
        let v1 = buf[1] as u32;
        let v2 = buf[2] as u32;
        let v3 = buf[3] as u32;
        (v3 << 24) +
        (v2 << 16) +
        (v1 << 8) +
        v0
    }
    
    pub fn decode_u64(buf: &[u8]) -> u64 {
        let v0 = buf[0] as u64;
        let v1 = buf[1] as u64;
        let v2 = buf[2] as u64;
        let v3 = buf[3] as u64;
        let v4 = buf[4] as u64;
        let v5 = buf[5] as u64;
        let v6 = buf[6] as u64;
        let v7 = buf[7] as u64;
        (v7 << 56) +
        (v6 << 48) +
        (v5 << 40) +
        (v4 << 32) +
        (v3 << 24) +
        (v2 << 16) +
        (v1 << 8) +
        v0
    }
    
    pub fn decode_f64(buf: &[u8]) -> f64 {
        let v = decode_u64(buf);
        f64::from_bits(v)
    }
    
}

pub fn parse_proto(buf: &[u8]) -> StateResp {

    let resp = StateResp {
        program_state: le::decode_u8(buf),
        error_code: le::decode_u8(&buf[1..]),
        robot_mode: le::decode_u8(&buf[2..]),

        jt_cur_pos0: le::decode_f64(&buf[3..]),
        jt_cur_pos1: le::decode_f64(&buf[11..]),
        jt_cur_pos2: le::decode_f64(&buf[19..]),
        jt_cur_pos3: le::decode_f64(&buf[27..]),
        jt_cur_pos4: le::decode_f64(&buf[35..]),
        jt_cur_pos5: le::decode_f64(&buf[43..]),
        
        tl_cur_pos0: le::decode_f64(&buf[51..]),
        tl_cur_pos1: le::decode_f64(&buf[59..]),
        tl_cur_pos2: le::decode_f64(&buf[67..]),
        tl_cur_pos3: le::decode_f64(&buf[75..]),
        tl_cur_pos4: le::decode_f64(&buf[83..]),
        tl_cur_pos5: le::decode_f64(&buf[91..]),

        tool_num: le::decode_u32(&buf[99..]),

        jt_cur_tor0: le::decode_f64(&buf[103..]),
        jt_cur_tor1: le::decode_f64(&buf[111..]),
        jt_cur_tor2: le::decode_f64(&buf[119..]),
        jt_cur_tor3: le::decode_f64(&buf[127..]),
        jt_cur_tor4: le::decode_f64(&buf[135..]),
        jt_cur_tor5: le::decode_f64(&buf[143..]),

        program_name: decode_cstr(&buf[151..], 20),
        prog_total_line: le::decode_u8(&buf[171..]),
        prog_cur_line: le::decode_u8(&buf[172..]),

        cl_dgt_output_h: le::decode_u8(&buf[173..]),
        cl_dgt_output_l: le::decode_u8(&buf[174..]),
        tl_dgt_output_l: le::decode_u8(&buf[175..]),

        cl_dgt_input_h: le::decode_u8(&buf[176..]),
        cl_dgt_input_l: le::decode_u8(&buf[177..]),
        tl_dgt_input_l: le::decode_u8(&buf[178..]),

        ft_data0: le::decode_f64(&buf[179..]),
        ft_data1: le::decode_f64(&buf[187..]),
        ft_data2: le::decode_f64(&buf[195..]),
        ft_data3: le::decode_f64(&buf[203..]),
        ft_data4: le::decode_f64(&buf[211..]),
        ft_data5: le::decode_f64(&buf[219..]),

        ft_act_status: le::decode_u8(&buf[227..]),
        emergency_stop: le::decode_u8(&buf[228..]),
        robot_motion_done: le::decode_u32(&buf[229..]),
        gripper_motion_done: le::decode_u8(&buf[233..])
    };

    resp
}

pub fn check_sum(buf: &[u8]) -> u16 {
    let mut s = 0;
    for v in buf {
        s += *v as u16;
    }

    s
}